%% a)关节空间三次项多项式
figure('name','三次多项式');
q3_s = 120; q3_f = 60;
t3_s=0; t3_f = 1;
v3_s = 0;v3_f = 0;


%计算系数
a0_3 = q3_s;
a1_3 = 0;
a2_3 = (3/t3_f^2)*(q3_f - q3_s);
a3_3 = (-2/t3_f^3)*(q3_f - q3_s);
j = 1;
for tc = 0: 0.01: 1
   q_3(j) = a0_3 + a1_3*tc + a2_3*tc^2 + a3_3*tc^3; %角度变化函数
   qd_3(j) = a1_3 + 2*a2_3*tc + 3*a3_3*tc^2;%角速度
   qdd_3(j) = 2*a2_3 + 6*a3_3*tc;%角加速度
   qddd_3(j) = 6*a3_3;%加速度变化率
   j = j + 1;
end
subplot(4,1,1),plot([0:0.01:1], q_3,'r'),xlabel('时间(t)'),ylabel('角度(°)');grid on;
subplot(4,1,2),plot([0:0.01:1], qd_3,'b'),xlabel('时间(t)'),ylabel('速度(°/s)');grid on;
subplot(4,1,3),plot([0:0.01:1], qdd_3,'g'),xlabel('时间(t)'),ylabel('加速度(°/s^2)');grid on;
subplot(4,1,4),plot([0:0.01:1], qddd_3,'k'),xlabel('时间(t)'),ylabel('加速度变化率');grid on;





  

%笛卡尔规划    
T0 = robot.fkine([0 0 0 0 0 0]);
Tf = robot.fkine([20/180*pi 20/180*pi 20/180*pi 20/180*pi 20/180*pi 20/180*pi]);
%利用 ctraj 在笛卡尔空间规划轨迹
step = 30;
Tc = ctraj(T0,Tf,step);
Tjtraj = transl(Tc);
plot2(Tjtraj,'r');
grid on;
title('T0到Tf笛卡尔空间轨迹');


% for A = 1:100
%     q(A,:) = robot.ikine(Tc(:,:,A));
% end
robot.plot(robot.ikine(Tc(:,:,1)),'trail','b')%绘制轨迹

%自编直线轨迹
xb=355.0;yb=0.0;zb=-376.0;
xf=xb+250;yf=yb+200;zf=zb+700;
S=sqrt(pow2(xf-xb)+pow2(yf-yb)+pow2(zf-zb));
SI=S/30;
vx=(xf-xb)/S;vy=(yf-yb)/S;vz=(zf-zb)/S;
dx=vx*SI; dy=vy*SI;dz=vz*SI;


for A=0:30
   xt=xb+dx*A;yt=yb+dy*A;zt=zb+dz*A;
   tf =  [
         1         0         0         xt
         0        -1         0         yt
         0         0        -1         zt
         0         0         0         1];
    q(A+1,:) = robot.ikine(tf);
end


robot.plot(q,'trail','b')